#include "uart.h"
#include <stdio.h>
using namespace std;

UART::UART():Serial()
{
    ros::NodeHandle nh;
    nh.param<std::string>("Port",_port,"/dev/ttyUSB0");
    nh.param<int>("Baudrate",_baudrate,115200);
    _to = Timeout::simpleTimeout(100);

}

void UART::state_callback(const mavros_msgs::State &msg){
    current_state = msg;
}
void UART::land_callback(const std_msgs::Bool &msg){
    land_flag = msg.data;
    std::cout<<"--->land_flag"<<land_flag<<std::endl;

}

// header: 
//   seq: 341
//   stamp: 
//     secs: 1626663034
//     nsecs: 467172352
//   frame_id: "base_link"
// status: 
//   status: 0
//   service: 1
// latitude: 40.0869225
// longitude: 116.5436088
// altitude: 45.378806301
// position_covariance: [3.3124000000000002, 0.0, 0.0, 0.0, 3.3124000000000002, 0.0, 0.0, 0.0, 4.149369]
// position_covariance_type: 2
// ---

void UART::gps_callback(const sensor_msgs::NavSatFix &current_info)
{
    //send vehicle infomation to serial
    // std::cout<<"position_send:"<<current_info.pose.pose.position.x<<","<<current_info.pose.pose.position.y<<","<<current_info.pose.pose.position.z<<std::endl;
    // std::cout<<"velocity_send:"<<current_info.twist.twist.linear.x<<","<<current_info.twist.twist.linear.y<<","<<current_info.twist.twist.linear.z<<std::endl;
    int current_latitude= current_info.latitude*10000000;
    int current_longitude= current_info.longitude*10000000;
    int current_altitude= current_info.altitude*10000000;

    int current_velx = 0;
    int current_vely = 0;
    int current_velz = 0;


    Int2char current_pointx_union, current_pointy_union, current_pointz_union, current_velx_union, current_vely_union, current_velz_union;
    current_pointx_union.Int = current_latitude;
    current_pointy_union.Int = current_longitude;
    current_pointz_union.Int = current_altitude;

    current_velx_union.Int = current_velx;
    current_vely_union.Int = current_vely;
    current_velz_union.Int = current_velz;


    //header and payload
    // [0] [1]header: 0xb5 0x62
    //[2]source: 0xa1(master) 0xb2(follower)
    //[3]target: 0xa1(master) 0xb2(follower)
    //[4] type: 0xa1(local) 0xb2(global)
    //[5] length of the whole message
    //[6] [7] [8] [9] system id : vehicle1:0x01,0x01,0x01,0x01  vechile2:0x01,0x01,0x01,0x02   .....each vehicle is different,be careful

    uint8_t  temp1[38] = {0xb5,0x62,0xa1,0xa1,0xb2,0x26, 0x01,0x01,0x01,0x01};
    temp1[10] = current_pointx_union.Char[0];
    temp1[11] = current_pointx_union.Char[1];
    temp1[12] = current_pointx_union.Char[2];
    temp1[13] = current_pointx_union.Char[3];

    temp1[14] = current_pointy_union.Char[0];
    temp1[15] = current_pointy_union.Char[1];
    temp1[16] = current_pointy_union.Char[2];
    temp1[17] = current_pointy_union.Char[3];

    temp1[18] = current_pointz_union.Char[0];
    temp1[19] = current_pointz_union.Char[1];
    temp1[20] = current_pointz_union.Char[2];
    temp1[21] = current_pointz_union.Char[3];

    temp1[22] = current_velx_union.Char[0];
    temp1[23] = current_velx_union.Char[1];
    temp1[24] = current_velx_union.Char[2];
    temp1[25] = current_velx_union.Char[3];

    temp1[26] = current_vely_union.Char[0];
    temp1[27] = current_vely_union.Char[1];
    temp1[28] = current_vely_union.Char[2];
    temp1[29] = current_vely_union.Char[3];

    temp1[30] = current_velz_union.Char[0];
    temp1[31] = current_velz_union.Char[1];
    temp1[32] = current_velz_union.Char[2];
    temp1[33] = current_velz_union.Char[3];

    //command list: 0x01->armed    0x02->disarmed &&land_flag=false  0x03->disarmed&&land_flag=true  
    if(!current_state.armed && land_flag==false)
    {
        temp1[34] = 0x02;
    }
    else if(current_state.armed){
        temp1[34] = 0x01; 
    }
    else if(!current_state.armed && land_flag==true)
    {
        temp1[34] = 0x03;
    }
    

    uint8_t crc = 0;

    for(uint8_t i=0;i<35;i++)
    {
        crc += temp1[i];
    }
    temp1[35] = crc;


    // crc check
    temp1[36] = 0x0d;
    temp1[37] = 0x0a;


    write(temp1, 38);

}

void UART::odometry_callback(const nav_msgs::Odometry &current_info)
{
    //send vehicle infomation to serial
    // std::cout<<"position_send:"<<current_info.pose.pose.position.x<<","<<current_info.pose.pose.position.y<<","<<current_info.pose.pose.position.z<<std::endl;
    // std::cout<<"velocity_send:"<<current_info.twist.twist.linear.x<<","<<current_info.twist.twist.linear.y<<","<<current_info.twist.twist.linear.z<<std::endl;
    int current_pointx= current_info.pose.pose.position.x*100;
    int current_pointy= current_info.pose.pose.position.y*100;
    int current_pointz= current_info.pose.pose.position.z*100;

    int current_velx = current_info.twist.twist.linear.x*100;
    int current_vely = current_info.twist.twist.linear.y*100;
    int current_velz = current_info.twist.twist.linear.z*100;


    Int2char current_pointx_union, current_pointy_union, current_pointz_union, current_velx_union, current_vely_union, current_velz_union;
    current_pointx_union.Int = current_pointx;
    current_pointy_union.Int = current_pointy;
    current_pointz_union.Int = current_pointz;

    current_velx_union.Int = current_velx;
    current_vely_union.Int = current_vely;
    current_velz_union.Int = current_velz;
    // printf("current_pointx_union.Char[0]: %08x\n", current_pointx_union.Char[0]);
    // printf("current_pointx_union.Char[1]: %08x\n", current_pointx_union.Char[1]);
    // printf("current_pointx_union.Char[2]: %08x\n", current_pointx_union.Char[2]);
    // printf("current_pointx_union.Char[3]: %08x\n", current_pointx_union.Char[3]);


    //header and payload
    // [0] [1]header: 0xb5 0x62
    //[2]source: 0xa1(master) 0xb2(follower)
    //[3]target: 0xa1(master) 0xb2(follower)
    //[4] type: 0xa1(local) 0xb2(global)
    //[5] length of the whole message
    //[6] [7] [8] [9] system id : vehicle1:0x01,0x01,0x01,0x01  vechile2:0x01,0x01,0x01,0x02   .....each vehicle is different

    uint8_t  temp1[38] = {0xb5,0x62,0xa1,0xb2,0xa1,0x26, 0x01,0x01,0x01,0x01};
    temp1[10] = current_pointx_union.Char[0];
    temp1[11] = current_pointx_union.Char[1];
    temp1[12] = current_pointx_union.Char[2];
    temp1[13] = current_pointx_union.Char[3];

    temp1[14] = current_pointy_union.Char[0];
    temp1[15] = current_pointy_union.Char[1];
    temp1[16] = current_pointy_union.Char[2];
    temp1[17] = current_pointy_union.Char[3];

    temp1[18] = current_pointz_union.Char[0];
    temp1[19] = current_pointz_union.Char[1];
    temp1[20] = current_pointz_union.Char[2];
    temp1[21] = current_pointz_union.Char[3];

    temp1[22] = current_velx_union.Char[0];
    temp1[23] = current_velx_union.Char[1];
    temp1[24] = current_velx_union.Char[2];
    temp1[25] = current_velx_union.Char[3];

    temp1[26] = current_vely_union.Char[0];
    temp1[27] = current_vely_union.Char[1];
    temp1[28] = current_vely_union.Char[2];
    temp1[29] = current_vely_union.Char[3];

    temp1[30] = current_velz_union.Char[0];
    temp1[31] = current_velz_union.Char[1];
    temp1[32] = current_velz_union.Char[2];
    temp1[33] = current_velz_union.Char[3];

    //command list: 0x01->主机到主机    0x02->主机到从机   0x03->从机到从机
    temp1[34] = 0x01; //command prepared

    uint8_t crc = 0;

    for(uint8_t i=0;i<35;i++)
    {
        crc += temp1[i];
    }
    temp1[35] = crc;


    // crc check
    temp1[36] = 0x0d;
    temp1[37] = 0x0a;


    write(temp1, 38);

}





void UART::OpenSerial()
{
    try {
        this->open();
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM("Unable to open port.");
        return;
    }

    if(this->isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return;
    }
}

uint8_t UART::CrcCheck(std::vector<uint8_t> buff,int len)
{
    uint8_t crc = 0;

    for(uint8_t i=0;i<len;i++)
    {
        crc += buff[i];
    }

    return crc;
}


void UART::RunMain()
{   
    
    uint8_t package[256];

    uint8_t rx_head = 0;
    uint8_t rx_wr_index = 0;
    uint8_t b_rx_over = 0;
    serial_process_t _serial_process;
    OpenSerial();
    ros::Rate loop_rate(10);

    ros::NodeHandle nh2;
    //advertise是接收串口数据，发送到ros
    //subscribe是接收ros数据，发送到串口

    ros::Publisher _pose_pub2 = nh2.advertise<nav_msgs::Odometry>("/posevel/fromothermaster",10);
    // ros::Subscriber _traj_sub2 = nh2.subscribe("/mavros/local_position/odom",10,&UART::odometry_callback,this);

    ros::Publisher _global_pub2 = nh2.advertise<sensor_msgs::NavSatFix>("/gps/fromothermaster",10);
    ros::Subscriber _global_sub2 = nh2.subscribe("/mavros/global_position/global",10,&UART::gps_callback,this);
    ros::Subscriber _state_sub2 = nh2.subscribe("/mavros/state",10,&UART::state_callback,this);
    ros::Subscriber _land_sub2 = nh2.subscribe("/thisvehicle/ifland",10,&UART::land_callback,this);

    while(ros::ok())
    {
        //read serial
        size_t n = this->available();

        uint8_t buffer[256];
        n = this->read(buffer,n);//must! n<=256
        uint8_t b[256];
        for(uint8_t i=0;i<n;i++)
        {
            b[i] = buffer[i];
        }

        for(uint8_t i=0;i<n;i++)
        {
            if(b[i]==0xb5)
            {
                rx_head = 1;
                package[rx_wr_index++] = b[i];
            }
            else if(b[i]==0x62)
            {
                if(rx_head)
                {
                    rx_wr_index = 0;
                    b_rx_over = 0;
                }
                else
                {
                    package[rx_wr_index++] = b[i];
                    rx_head = 0;
                }

            }
            else
            {
                package[rx_wr_index++] = b[i];
                rx_head = 0;
                if(rx_wr_index>32) //>4*8 zanding
                {
                    if((rx_wr_index+2) == package[3] )
                    {

                        std::cout<<"get whole package."<<std::endl;
                        b_rx_over = 1;

                        _serial_process.rx_buffer.clear();
                        _serial_process.rx_buffer.push_back(0xb5);
                        _serial_process.rx_buffer.push_back(0x62);

                        for(uint8_t i=2; i<package[3];i++)
                        {
                            _serial_process.rx_buffer.push_back(package[i-2]);
                        }
                        _serial_process.index = package[3];
                        _serial_process.rx_over = 1;
                        memset(package,0,256);
                    }
                }
            }
            if(rx_wr_index>200)
                rx_wr_index=200;
        }

        if(_serial_process.rx_over==1)
        {
            std::vector<uint8_t> buffer;
            for(uint8_t i=0;i<_serial_process.index;i++)
            {

                buffer.push_back(_serial_process.rx_buffer[i]);
            }
            // std::cout<<(int)buffer[0]<<std::endl;
            if(buffer[0]==0xb5 && buffer[1]==0x62 && buffer[_serial_process.index-2]==0x0d && buffer[_serial_process.index-1]==0x0a)
            {
                std::cout<<"check header successfully!"<<std::endl;
                //crc check
                uint8_t crc = CrcCheck(buffer,_serial_process.index-3);
                if(crc == buffer[_serial_process.index-3])
                {
                    uint8_t temp[_serial_process.index];
                    for(uint8_t i=0;i<_serial_process.index;i++)
                    {
                        temp[i] = buffer[i];
                    }
                    std::cout<<"check crc successfully!"<<std::endl;
                    //check which vehicle the message from
                    int vehicle_id = temp[9];

                    if(temp[4] == 0xa1)
                    {
                        memcpy(&_positionRec,temp+10,25);
                        nav_msgs::Odometry msg1;
                        msg1.pose.pose.position.x = _positionRec.pos_x/100.0;
                        msg1.pose.pose.position.y = _positionRec.pos_y/100.0;
                        msg1.pose.pose.position.z = _positionRec.pos_z/100.0;
                        msg1.twist.twist.linear.x = _positionRec.vel_x/100.0;
                        msg1.twist.twist.linear.y = _positionRec.vel_y/100.0;
                        msg1.twist.twist.linear.z = _positionRec.vel_z/100.0;
                        msg1.pose.covariance[0] = _positionRec.command;
                        msg1.pose.covariance[1] = vehicle_id;
                        std::cout<<"position_receive:"<<msg1.pose.pose.position.x<<","<<msg1.pose.pose.position.y<<","<<msg1.pose.pose.position.z<<std::endl;
                        std::cout<<"velocity_receive:"<<msg1.twist.twist.linear.x<<","<<msg1.twist.twist.linear.y<<","<<msg1.twist.twist.linear.z<<std::endl;
                        _pose_pub2.publish(msg1);

                    }
                    else if(temp[4] == 0xb2)
                    {
                        memcpy(&_gpsRec,temp+10,25);
                        sensor_msgs::NavSatFix msg2;
                        msg2.latitude = _gpsRec.latitude/10000000.0;
                        msg2.longitude = _gpsRec.longitude/10000000.0;
                        msg2.altitude = _gpsRec.altitude/10000000.0;
                        msg2.position_covariance[0] = _gpsRec.command;
                        msg2.position_covariance[1] = vehicle_id;

                        std::cout<<"gps_receive:"<<msg2.latitude<<","<<msg2.longitude<<","<<msg2.altitude<<std::endl;
                        _global_pub2.publish(msg2);
                    }


                }
            }

            _serial_process.rx_over = 0;
        }


        ros::spinOnce();
        loop_rate.sleep();
    }
}
